/**
 *
 * Nand Software ECC support
 * copy from Linux kernel
 *
 */

#include <core/errno.h>
#include <core/types.h>
#include <flash/nand.h>
#include <flash/nand_ecc.h>


static const BYTE nand_ecc_precalc_table[] = {
	0x00, 0x55, 0x56, 0x03, 0x59, 0x0c, 0x0f, 0x5a, 0x5a, 0x0f, 0x0c, 0x59, 0x03, 0x56, 0x55, 0x00,
	0x65, 0x30, 0x33, 0x66, 0x3c, 0x69, 0x6a, 0x3f, 0x3f, 0x6a, 0x69, 0x3c, 0x66, 0x33, 0x30, 0x65,
	0x66, 0x33, 0x30, 0x65, 0x3f, 0x6a, 0x69, 0x3c, 0x3c, 0x69, 0x6a, 0x3f, 0x65, 0x30, 0x33, 0x66,
	0x03, 0x56, 0x55, 0x00, 0x5a, 0x0f, 0x0c, 0x59, 0x59, 0x0c, 0x0f, 0x5a, 0x00, 0x55, 0x56, 0x03,
	0x69, 0x3c, 0x3f, 0x6a, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6a, 0x3f, 0x3c, 0x69,
	0x0c, 0x59, 0x5a, 0x0f, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00, 0x55, 0x0f, 0x5a, 0x59, 0x0c,
	0x0f, 0x5a, 0x59, 0x0c, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56, 0x0c, 0x59, 0x5a, 0x0f,
	0x6a, 0x3f, 0x3c, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3c, 0x3f, 0x6a,
	0x6a, 0x3f, 0x3c, 0x69, 0x33, 0x66, 0x65, 0x30, 0x30, 0x65, 0x66, 0x33, 0x69, 0x3c, 0x3f, 0x6a,
	0x0f, 0x5a, 0x59, 0x0c, 0x56, 0x03, 0x00, 0x55, 0x55, 0x00, 0x03, 0x56, 0x0c, 0x59, 0x5a, 0x0f,
	0x0c, 0x59, 0x5a, 0x0f, 0x55, 0x00, 0x03, 0x56, 0x56, 0x03, 0x00, 0x55, 0x0f, 0x5a, 0x59, 0x0c,
	0x69, 0x3c, 0x3f, 0x6a, 0x30, 0x65, 0x66, 0x33, 0x33, 0x66, 0x65, 0x30, 0x6a, 0x3f, 0x3c, 0x69,
	0x03, 0x56, 0x55, 0x00, 0x5a, 0x0f, 0x0c, 0x59, 0x59, 0x0c, 0x0f, 0x5a, 0x00, 0x55, 0x56, 0x03,
	0x66, 0x33, 0x30, 0x65, 0x3f, 0x6a, 0x69, 0x3c, 0x3c, 0x69, 0x6a, 0x3f, 0x65, 0x30, 0x33, 0x66,
	0x65, 0x30, 0x33, 0x66, 0x3c, 0x69, 0x6a, 0x3f, 0x3f, 0x6a, 0x69, 0x3c, 0x66, 0x33, 0x30, 0x65,
	0x00, 0x55, 0x56, 0x03, 0x59, 0x0c, 0x0f, 0x5a, 0x5a, 0x0f, 0x0c, 0x59, 0x03, 0x56, 0x55, 0x00
};


int NandEccCalculate(struct NandFlash *pNandFlash, const BYTE *dat, BYTE *ecc_code)
{
	BYTE idx, reg1, reg2, reg3, tmp1, tmp2;
	int i;


	reg1 = reg2 = reg3 = 0;


	for(i = 0; i < 256; i++) {
	
		idx = nand_ecc_precalc_table[*dat++];
		reg1 ^= (idx & 0x3f);

	
		if (idx & 0x40) {
			reg3 ^= (BYTE) i;
			reg2 ^= ~((BYTE) i);
		}
	}


	tmp1  = (reg3 & 0x80) >> 0; 
	tmp1 |= (reg2 & 0x80) >> 1; 
	tmp1 |= (reg3 & 0x40) >> 1; 
	tmp1 |= (reg2 & 0x40) >> 2; 
	tmp1 |= (reg3 & 0x20) >> 2; 
	tmp1 |= (reg2 & 0x20) >> 3; 
	tmp1 |= (reg3 & 0x10) >> 3; 
	tmp1 |= (reg2 & 0x10) >> 4; 

	tmp2  = (reg3 & 0x08) << 4; 
	tmp2 |= (reg2 & 0x08) << 3; 
	tmp2 |= (reg3 & 0x04) << 3; 
	tmp2 |= (reg2 & 0x04) << 2; 
	tmp2 |= (reg3 & 0x02) << 2; 
	tmp2 |= (reg2 & 0x02) << 1; 
	tmp2 |= (reg3 & 0x01) << 1; 
	tmp2 |= (reg2 & 0x01) << 0; 


#ifdef CONF_FLASH_NAND_ECC_SMC
	ecc_code[0] = ~tmp2;
	ecc_code[1] = ~tmp1;
#else
	ecc_code[0] = ~tmp1;
	ecc_code[1] = ~tmp2;
#endif
	ecc_code[2] = ((~reg1) << 2) | 0x03;

	return 0;
}


static inline int countbits(UINT32 byte)
{
	int res = 0;

	for (; byte; byte >>= 1)
		res += byte & 0x01;

	return res;
}


int NandEccCorrect(struct NandFlash *pNandFlash, BYTE *dat, BYTE *read_ecc, BYTE *calc_ecc)
{
	BYTE s0, s1, s2;

#ifdef CONF_FLASH_NAND_ECC_SMC
	s0 = calc_ecc[0] ^ read_ecc[0];
	s1 = calc_ecc[1] ^ read_ecc[1];
	s2 = calc_ecc[2] ^ read_ecc[2];
#else
	s1 = calc_ecc[0] ^ read_ecc[0];
	s0 = calc_ecc[1] ^ read_ecc[1];
	s2 = calc_ecc[2] ^ read_ecc[2];
#endif
	if ((s0 | s1 | s2) == 0)
		return 0;


	if( ((s0 ^ (s0 >> 1)) & 0x55) == 0x55 &&
		((s1 ^ (s1 >> 1)) & 0x55) == 0x55 &&
		((s2 ^ (s2 >> 1)) & 0x54) == 0x54)
	{
		UINT32 byteoffs, bitnum;

		byteoffs = (s1 << 0) & 0x80;
		byteoffs |= (s1 << 1) & 0x40;
		byteoffs |= (s1 << 2) & 0x20;
		byteoffs |= (s1 << 3) & 0x10;

		byteoffs |= (s0 >> 4) & 0x08;
		byteoffs |= (s0 >> 3) & 0x04;
		byteoffs |= (s0 >> 2) & 0x02;
		byteoffs |= (s0 >> 1) & 0x01;

		bitnum = (s2 >> 5) & 0x04;
		bitnum |= (s2 >> 4) & 0x02;
		bitnum |= (s2 >> 3) & 0x01;

		dat[byteoffs] ^= (1 << bitnum);

		return 1;
	}

	if(countbits(s0 | ((UINT32)s1 << 8) | ((UINT32)s2 <<16)) == 1)
		return 1;

	return -EBADMSG;
} 
